#include "mbed.h"
#include "Gyroscope/Gyroscope.h"
#include "Accelerometre/ADXL345_I2C.h"

int main() {
    
    Gyroscope gyro(p9, p10);
    ADXL345_I2C accelerometer(p9, p10); 
    Serial pc(USBTX, USBRX);
    
    int readings[3] = {0, 0, 0};
    
    pc.printf("Starting ADXL345...\n\r");
    wait(.001);
    pc.printf("Device ID is: 0x%02x\n\r", accelerometer.getDeviceID());
    wait(.001);
    
    if (accelerometer.setPowerControl(0x00)){
        pc.printf("didn't intitialize power control\n\r"); 
        return 0;  
    }
    //Full resolution, +/-16g, 4mg/LSB.
    wait(.001);
    
    if(accelerometer.setDataFormatControl(0x0B)){
        pc.printf("didn't set data format\n\r");
        return 0; 
    }
    wait(.001);
    
    //3.2kHz data rate.
    if(accelerometer.setDataRate(ADXL345_3200HZ)){
        pc.printf("didn't set data rate\n\r");
        return 0;    
    }
    wait(.001);
    
    //Measurement mode.
    
    if(accelerometer.setPowerControl(MeasurementMode)) {
        pc.printf("didn't set the power control to measurement\n\r"); 
        return 0;  
    } 
    
    pc.printf("Starting ITG-3200...\n\r");   
    
    gyro.Init();
    gyro.setSMPLRT_DIV(SRD_NONE);
    gyro.setDLPF_FS(DLPF_BW098_SR1|FS_RANGE2000);
    gyro.setINT_CFG(INT_DATA_AVBLE_EN);
    gyro.setPWR_MGM(PW_INTERNALOSC); 
    
    while(1){
        
        gyro.Read_Sensors();
        pc.printf("GYRO(x: %i, y: %i, z: %i, t: %i)\n\r",gyro.getX(),
                  gyro.getY(),
                  gyro.getZ(),
                  gyro.getTemp()); 
        
        accelerometer.getOutput(readings);
        
        pc.printf("ACCEL(x: %i, y: %i, z: %i)\n\r", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);  
        
        wait(0.1);
    }
}
